﻿using Microsoft.Extensions.Configuration;
using Microsoft.Extensions.Logging;
using ServoDrivesLib.Controls;
using ServoDrivesLibAbstract;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace ServoDrivesLib.Device
{
    public class JodellControl : ForceModeDeviceControl
    {
        public JodellControl(CardControl? servo, int axisId) : base(servo)
        {
            AxisId = axisId;
        }
        public override int OpenCia402()
        {
            int ret = 0;
            ret = servo?.SDOWrite(AxisId + 1, 0x300D, 0, 16, 1) ?? -1;
            return ret;
        }
        public override int CloseCia402()
        {
            int ret = 0;
            ret = servo?.SDOWrite(AxisId + 1, 0x300D, 0, 16, 0) ?? -1;
            return ret;
        }

        public int SetElectricity(double value)
        {
            int ret = 0;
            ret = servo?.SDOWrite(AxisId + 1, 0x3006, 0, 32, (int)(value * 10000)) ?? -1;
            return ret;
        }


        public int SetPositionMode(int value)
        {
            int ret = 0;
            ret = servo?.SDOWrite(AxisId + 1, 0x300E, 0, 8, value) ?? -1;
            return ret;
        }

        public override int SetControl(int value)
        {
            int ret = 0;
            ret = servo?.SDOWrite(AxisId + 1, 0x3000, 0, 16, value) ?? -1;
            return ret;
        }

        public override int GetStatus()
        {
            int ret = 0;
            servo?.SDORead(AxisId + 1, 0x3001, 0, 16, ref ret);
            return ret;
        }

        public int SetNoCia402Param(double downPos, double upPos, double speed, double acc, double hold, double electricity)
        {
            int ret = 0;
            ret = servo?.SDOWrite(AxisId + 1, 0x3002, 0, 32, (int)(10000 * downPos)) ?? -1;
            ret = servo?.SDOWrite(AxisId + 1, 0x3004, 0, 32, (int)(10000 * speed)) ?? -1;
            ret = servo?.SDOWrite(AxisId + 1, 0x3009, 0, 32, (int)(10000 * upPos)) ?? -1;
            ret = servo?.SDOWrite(AxisId + 1, 0x300B, 0, 32, (int)(10000 * acc)) ?? -1;
            ret = servo?.SDOWrite(AxisId + 1, 0x300A, 0, 32, (int)(10000 * hold)) ?? -1;
            ret = servo?.SDOWrite(AxisId + 1, 0x3006, 0, 32, (int)(10000 * electricity)) ?? -1;

            return ret;

        }
        public int GetParam(int index, int size)
        {
            int ret = 0;
            servo?.SDORead(AxisId + 1, index, 0, size, ref ret);
            return ret;
        }


    }

    internal static class FloatEx
    {
        public static int FloatToInt32(this float nums)
        {
            byte[] bytes = BitConverter.GetBytes(nums);
            int ret = BitConverter.ToInt32(bytes, bytes.Length);
            return ret;
        }
    }
}
